/**
 * @file   a_star.cpp
 * @brief  a star算法
 * @encode UTF-8
 * @log    date        author      notes
 *         2021.09.01  Jimmy       first version
 */
#include "a_star.h"
#include "utils/date_time.h"
#include "debug_log/log.h"

const uint8_t ASTAR_DILATE_SIZE        = 3;
const uint8_t ASTAR_CW_COST_TABEL_SIZE = 4;     /* 靠墙成本 */
const uint8_t ASTAR_EXPEND_CNT         = 4;

/* 靠墙惩罚成本 */
const uint8_t closeWallPunishCost[ASTAR_CW_COST_TABEL_SIZE + 1] = {32, 16, 8, 4, 2};
const int8_t  xOffset[8]   = { 1, -1, 0, 0, 1, 1, -1, -1 };
const int8_t  yOffset[8]   = { 0, 0, -1, 1, 1, -1, 1, -1 };
const float   astarCost[8] = { 1.0, 1.0, 1.0, 1.0, 1.4, 1.4, 1.4, 1.4 };

/**
 * @brief  Construct a new AStar::AStar object
 * @author Jimmy 
 * @date   2021.09.01
 */
AStar::AStar()
{
    mStartOpen = std::make_unique<Heap>      ();
    mEndOpen   = std::make_unique<Heap>      ();
    mClose     = std::make_unique<CloseList> ();
}

/**
 * @brief  Destroy the AStar::AStar object
 * @author Jimmy 
 * @date   2021.09.01
 */
AStar::~AStar()
{
}

/**
 * @brief  地图更新
 * @param  mapData          
 * @author Jimmy 
 * @date   2021.09.03
 */
void AStar::updateMap(const MapData & mapData)
{
    mWidth   = mapData.param.width;
    mHeight  = mapData.param.height;
    mMapSize = mWidth * mHeight;

    mMapByte.resize(mMapSize, 0);
    mMapByte = mapData.map;

    for (int y = 0; y < mHeight; y ++)
    {
        int wid = y * mWidth;

        for (int x = 0; x < mWidth; x ++)
        {
            int i = wid + x;
            /* 值低于255的，全部认为是障碍物 */
            if (mMapByte[i] < 127)
            {
                mMapByte[i] = 10;
            }
            else
            {
                mMapByte[i] = 255;
            }
        }
    }
}

/**
 * @brief  地图更新
 * @param  mapData          
 * @param  width            
 * @param  height           
 * @param  xMin             
 * @param  yMin             
 * @param  xMax             
 * @param  yMax             
 * @param  do_erode         
 * @author Jimmy 
 * @date   2021.09.03
 */
void AStar::updateMap(const MapData & mapData, uint16_t width, uint16_t height, int xMin, int yMin, int xMax, int yMax, bool do_erode)
{
    mWidth  = width;
    mHeight = height;
    mMapSize = width * height;

    log_shell::LOG_INFO("update map:(%d, %d)\r\n", mWidth, mHeight);
    mMapByte.resize(mMapSize, 0);
    mMapByte = mapData.map;

    xMin = std::max(0, xMin);
    yMin = std::max(0, yMin);
    xMax = std::min(width - 1,  xMax);
    yMax = std::min(height - 1, yMax);

    int deltaPos[] = {-1, 1, -width, width};
    uint8_t deltaSize = sizeof(deltaPos) / sizeof(deltaPos[0]);

    /* 像素膨胀 */
    if (do_erode)
    {
        for (auto dis = 0; dis < ASTAR_DILATE_SIZE; dis ++)
        {
            for (auto yy = yMin + 1; yy <= yMax - 1; yy ++)
            {
                auto yy_widht = yy * width;

                for (auto xx = xMin + 1; xx <= xMax - 1; xx ++)
                {
                    auto i = yy_widht + xx;

                    if (mMapByte[i] == dis)
                    {
                        for (auto j = 0; j < deltaSize; j ++)
                        {
                            if (mMapByte[i + deltaPos[j]] >= 127)
                            {
                                mMapByte[i + deltaPos[j]] = dis + 1;
                            }
                        }
                    }
                }
            }
        }

        /* 膨胀后过滤 */
        for (auto y = yMin; y <= yMax; y ++)
        {
            int wid = y * mWidth;

            for (auto x = xMin; x <= xMax; x ++)
            {
                int i = wid + x;
                /* 值低于255的，全部认为是障碍物 */
                if (mMapByte[i] < MAP_PIXEL_CLR)
                {
                    mMapByte[i] = MAP_PIXEL_OBSTACLE;
                }
            }
        }
    }

    /* 累加到G上的额外成本 */
    for (auto dilate_v = 0; dilate_v < ASTAR_CW_COST_TABEL_SIZE; dilate_v ++)
    {
        for (auto yy = yMin + 1; yy <= yMax - 1; yy ++)
        {
            auto yy_widht = yy * width;

            for (auto xx = xMin + 1; xx <= xMax - 1; xx ++)
            {
                auto i = yy_widht + xx;

                if (mMapByte[i] == dilate_v)
                {
                    for (auto j = 0; j < deltaSize; j ++)
                    {
                        if (mMapByte[i + deltaPos[j]] = MAP_PIXEL_CLR)
                        {
                            mMapByte[i + deltaPos[j]] = dilate_v + 1;
                        }
                    }
                }
            }
        }
    }
}

/**
 * @brief  路径规划，双边
 * @param  start            
 * @param  dest             
 * @param  map              
 * @param  path             
 * @return a_star::astar_status_t 
 * @author Jimmy 
 * @date   2021.09.01
 */
a_star::astar_status_t AStar::pathing(Point &start, Point &dest, std::vector<Point> &path)
{
    log_shell::LOG_INFO("AStar Pathing\r\n");

    log_shell::LOG_INFO("start(%d, %d), dest(%d, %d)\r\n", start.x, start.y, dest.x, dest.y);
    TimeInterval costTime;
    costTime.mark();
    Point zero(0, 0);
    Point maxMap(mWidth, mHeight);

    if (!isPointIn(start, zero, maxMap))
    {
        log_shell::LOG_ERROR("a_star::ASS_START_TRAP\r\n");
        return a_star::ASS_START_TRAP;
    }

    if (!isPointIn(dest, zero, maxMap))
    {
        log_shell::LOG_ERROR("a_star::ASS_END_TRAP\r\n");
        return a_star::ASS_END_TRAP;
    }

    if (!canPass(start, MAP_PIXEL_CLR))
    {
        log_shell::LOG_ERROR("a_star::ASS_START_TRAP\r\n");
        return a_star::ASS_START_TRAP;
    }

    mClose->init(mWidth, mHeight);
    mClose->addToList(9, 1, start);
    mClose->addToList(9, 2, dest);

    mStartOpen->clear();
    AStarPoint startPoint;
    startPoint.pos = start;
    startPoint.g = 0;
    startPoint.h = calcH(start, dest);
    startPoint.f = startPoint.g + startPoint.h;
    mStartOpen->push(startPoint);

    mEndOpen->clear();
    AStarPoint endPoint;
    endPoint.pos = dest;
    endPoint.g   = 0;
    endPoint.h   = calcH(dest, start);
    endPoint.f   = endPoint.g + endPoint.h;
    mEndOpen->push(endPoint);

    bool isReachTarget = false;

    a_star::astar_status_t ret = a_star::ASS_OK;

    Point posM1, posM2;
    Point maxRPos;

    while((!mStartOpen->isEmpty()) && (!mEndOpen->isEmpty()) && (!isReachTarget))
    {
        AStarPoint curStartPoint = mStartOpen->popMin();

        /*  遍历方向
         *          |4|
         *   |2|   |cur|    |1|
         *          |3|
         */ 
        for (auto i = 0; i < 4; i ++)
        {
            Point nextPos(curStartPoint.pos.x + xOffset[i], curStartPoint.pos.y + yOffset[i]);
            int8_t index = 1;

            if (canPass(nextPos, MAP_PIXEL_CLR))
            {
                int addRet =  mClose->addToList(i, index, nextPos);
                if (addRet == CloseList::CLR_SUCCESS)
                {
                    AStarPoint nextPoint;
                    
                    nextPoint.g = curStartPoint.g + astarCost[i];
                    
                    int punishCostIndex = mMapByte[nextPos.y * mWidth + nextPos.x];
                    if (punishCostIndex <= 7)
                    {
                        /* 计算靠墙的惩罚成本 */
                        nextPoint.g += closeWallPunishCost[punishCostIndex];
                    }

                    nextPoint.h   = calcH(nextPos, dest);
                    nextPoint.f   = nextPoint.g + nextPoint.h;
                    nextPoint.pos = nextPos;
                    mStartOpen->push(nextPoint);
                }
                else 
                {
                    if (addRet == CloseList::CLR_CLOSED && isReachTarget == false)
                    {
                        isReachTarget = true;
                        posM1 = curStartPoint.pos;
                        posM2 = nextPos;
                    }
                    else
                    {
                        continue;
                    }
                }
            }
        }

        AStarPoint curEndPoint = mEndOpen->popMin();

        for (auto i = 0; i < 4; i ++)
        {
            Point nextPos(curEndPoint.pos.x + xOffset[i], curEndPoint.pos.y + yOffset[i]);
            int8_t index = 2;

            if (canPass(nextPos, MAP_PIXEL_CLR))
            {
                int addRet =  mClose->addToList(i, index, nextPos);
                if (addRet == CloseList::CLR_SUCCESS)
                {
                    AStarPoint nextPoint;
                    
                    nextPoint.g = curEndPoint.g + astarCost[i];
                    
                    int punishCostIndex = mMapByte[nextPos.y * mWidth + nextPos.x];
                    if (punishCostIndex <= 7)
                    {
                        nextPoint.g += closeWallPunishCost[punishCostIndex];
                    }

                    nextPoint.h   = calcH(nextPos, start);
                    nextPoint.f   = nextPoint.g + nextPoint.h;
                    nextPoint.pos = nextPos;
                    mEndOpen->push(nextPoint);
                }
                else 
                {
                    if (addRet == CloseList::CLR_CLOSED && isReachTarget == false)
                    {
                        isReachTarget = true;
                        posM2 = curEndPoint.pos;
                        posM1 = nextPos;
                    }
                    else
                    {
                        continue;
                    }
                }
            }
        }
    }

    if (mStartOpen->isEmpty())
    {
        ret = a_star::ASS_START_TRAP;
        log_shell::LOG_ERROR("ret = a_star::ASS_START_TRAP\r\n");
    }
    if (mEndOpen->isEmpty())
    {
        ret = a_star::ASS_END_TRAP;
        log_shell::LOG_ERROR("ret = a_star::ASS_END_TRAP\r\n");
    }

    std::vector<Point> tempPoints;

    if (isReachTarget)
    {
        /* 回溯路径 */
        std::vector<Point> startPoints;
        Point nowPoint = posM1;

        while (nowPoint.x != start.x || nowPoint.y != start.y)
        {
            startPoints.push_back(nowPoint);
            uint8_t dxdy = mClose->getDxDy(nowPoint);
            nowPoint.x -= xOffset[dxdy];
            nowPoint.y -= yOffset[dxdy];
        }

        startPoints.push_back(nowPoint);

        while (startPoints.size() > 0)
        {
            tempPoints.push_back(startPoints.back());
            startPoints.pop_back();
        }

        std::vector<Point> endPoints;
        nowPoint = posM2;

        while (nowPoint.x != dest.x || nowPoint.y != dest.y)
        {
            endPoints.push_back(nowPoint);
            uint8_t dxdy = mClose->getDxDy(nowPoint);
            nowPoint.x -= xOffset[dxdy];
            nowPoint.y -= yOffset[dxdy];
        }

        endPoints.push_back(nowPoint);

        for (size_t i = 0; i < endPoints.size(); i++)
        {
            tempPoints.push_back(endPoints[i]); 
        }
    }

    if (tempPoints.empty() == false)
    {
        path.clear();
        path.push_back(tempPoints[0]);
        
        for (auto i = 1; i < tempPoints.size() - 1; i ++)
        {
            if (!pointIsCanJump(path.back(), tempPoints[i], 4))
            // if (true)
            {
                path.push_back(tempPoints[i]);
            }            
        }
        path.push_back(tempPoints[tempPoints.size() - 1]);
    }

    log_shell::LOG_INFO(" AStar path finish\r\n");
    log_shell::LOG_INFO(" ┠ Star open list:%d\r\n", mStartOpen->size());
    log_shell::LOG_INFO(" ┠ End open list:%d\r\n", mEndOpen->size());
    log_shell::LOG_INFO(" ┠ Close list:%d\r\n", mClose->getCloseCount());
    log_shell::LOG_INFO(" ┗ Cost time:%f ms\r\n", (float)(costTime.interval()/1000.));
    mStartOpen->clear();
    mEndOpen->clear();

    return ret;
}

/**
 * @brief  路径规划，单边
 * @param  start            
 * @param  dest             
 * @param  path             
 * @return a_star::astar_status_t 
 * @author Jimmy 
 * @date   2021.09.18
 */
a_star::astar_status_t AStar::pathingWithSingle(Point & start, Point & dest, std::vector<Point> & path)
{
    log_shell::LOG_INFO("AStar Pathing With Single\r\n");

    log_shell::LOG_INFO("start(%d, %d), dest(%d, %d)\r\n", start.x, start.y, dest.x, dest.y);
    TimeInterval costTime;
    costTime.mark();
    Point zero(0, 0);
    Point maxMap(mWidth, mHeight);

    if (!isPointIn(start, zero, maxMap))
    {
        log_shell::LOG_ERROR("a_star::ASS_START_TRAP\r\n");
        return a_star::ASS_START_TRAP;
    }

    if (!isPointIn(dest, zero, maxMap))
    {
        log_shell::LOG_ERROR("a_star::ASS_END_TRAP\r\n");
        return a_star::ASS_END_TRAP;
    }

    if (!canPass(start, MAP_PIXEL_CLR))
    {
        log_shell::LOG_ERROR("a_star::ASS_START_TRAP\r\n");
        return a_star::ASS_START_TRAP;
    }

    mClose->init(mWidth, mHeight);
    mClose->addToList(9, 1, start);
    mClose->addToList(9, 2, dest);

    mStartOpen->clear();
    AStarPoint startPoint;
    startPoint.pos = start;
    startPoint.g = 0;
    startPoint.h = calcH(start, dest);
    startPoint.f = startPoint.g + startPoint.h;
    mStartOpen->push(startPoint);

    bool isReachTarget = false;

    a_star::astar_status_t ret = a_star::ASS_OK;

    Point posM1, posM2;
    Point maxRPos;

    while((!mStartOpen->isEmpty()) && (!isReachTarget))
    {
        AStarPoint curStartPoint = mStartOpen->popMin();

        /*  遍历方向
         *          |4|
         *   |2|   |cur|    |1|
         *          |3|
         */ 
        for (auto i = 0; i < ASTAR_EXPEND_CNT; i ++)
        {
            Point nextPos(curStartPoint.pos.x + xOffset[i], curStartPoint.pos.y + yOffset[i]);
            int8_t index = 1;

            if (canPass(nextPos, MAP_PIXEL_CLR))
            {
                int addRet =  mClose->addToList(i, index, nextPos);
                if (addRet == CloseList::CLR_SUCCESS)
                {
                    AStarPoint nextPoint;
                    
                    nextPoint.g = curStartPoint.g + astarCost[i];
                    
                    int punishCostIndex = mMapByte[nextPos.y * mWidth + nextPos.x];
                    if (punishCostIndex <= 7)
                    {
                        /* 计算靠墙的惩罚成本 */
                        nextPoint.g += closeWallPunishCost[punishCostIndex];
                    }

                    nextPoint.h   = calcH(nextPos, dest);
                    nextPoint.f   = nextPoint.g + nextPoint.h;
                    nextPoint.pos = nextPos;
                    mStartOpen->push(nextPoint);
                }
                else 
                {
                    if (addRet == CloseList::CLR_CLOSED && isReachTarget == false)
                    {
                        isReachTarget = true;
                        posM1 = curStartPoint.pos;
                        posM2 = nextPos;
                    }
                    else
                    {
                        continue;
                    }
                }
            }
        }

    }

    if (mStartOpen->isEmpty())
    {
        ret = a_star::ASS_START_TRAP;
        log_shell::LOG_ERROR("ret = a_star::ASS_START_TRAP\r\n");
    }

    std::vector<Point> tempPoints;

    if (isReachTarget)
    {
        /* 回溯路径 */
        std::vector<Point> startPoints;
        Point nowPoint = posM1;

        while (nowPoint.x != start.x || nowPoint.y != start.y)
        {
            startPoints.push_back(nowPoint);
            uint8_t dxdy = mClose->getDxDy(nowPoint);
            nowPoint.x -= xOffset[dxdy];
            nowPoint.y -= yOffset[dxdy];
        }

        startPoints.push_back(nowPoint);

        while (startPoints.size() > 0)
        {
            tempPoints.push_back(startPoints.back());
            startPoints.pop_back();
        }
    }

    if (tempPoints.empty() == false)
    {
        path.clear();
        path.push_back(tempPoints[0]);
        
        for (auto i = 1; i < tempPoints.size() - 1; i ++)
        {
            // if (!pointIsCanJump(path.back(), tempPoints[i], 4))
            if (true)
            {
                path.push_back(tempPoints[i]);
            }            
        }
        path.push_back(tempPoints[tempPoints.size() - 1]);
        path.push_back(dest);
    }

    log_shell::LOG_INFO(" AStar path finish\r\n");
    log_shell::LOG_INFO(" ┠ Star open list:%d\r\n", mStartOpen->size());
    log_shell::LOG_INFO(" ┠ End open list:%d\r\n", mEndOpen->size());
    log_shell::LOG_INFO(" ┠ Close list:%d\r\n", mClose->getCloseCount());
    log_shell::LOG_INFO(" ┗ Cost time:%f ms\r\n", (float)(costTime.interval()/1000.));
    mStartOpen->clear();
    mEndOpen->clear();

    return ret;
}


/**
 * @brief  计算H
 * @param  n_point          
 * @param  dest_point       
 * @return uint8_t 
 * @author Jimmy 
 * @date   2021.09.01
 */
uint8_t AStar::calcH(Point & n_point, Point & dest_point)
{
    /* 四邻域用曼哈顿距离计算 */
    return (abs(dest_point.x - n_point.x) + abs(dest_point.y - n_point.y));
    
    /* 八邻域用对角线距离计算 */
    uint8_t dx = n_point.x - dest_point.x;
    uint8_t dy = n_point.y - dest_point.y;
    uint8_t h = std::sqrt((dx * dx) + (dy * dy));

    return h;
}


/**
 * @brief  节点可通行
 * @param  point            
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2021.09.03
 */
bool AStar::canPass(Point & point, uint8_t ref)
{
    auto i = point.y * mWidth + point.x;

    if (mMapByte[i] < ref)
    {
        return false;
    }

    return true;
}

/**
 * @brief  判断点是否在地图内
 * @param  point            
 * @param  minPoint         
 * @param  maxPoint         
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2021.09.02
 */
bool AStar::isPointIn(Point & point, Point & minPoint, Point & maxPoint)
{
    if (point.x < minPoint.x || point.y < minPoint.y || point.x >= maxPoint.x || point.y >= maxPoint.y)
    {
        return false;
    }

    return true;
}

/**
 * @brief  点可以跳过？
 * @param  p1               
 * @param  p2               
 * @param  ref              
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2021.09.15
 * @note   从p1点开始，到p2点，判断p1到p2连线上所有的点是不是障碍物
 */
bool AStar::pointIsCanJump(Point p1, Point p2, uint8_t ref)
{
    int dx = p1.x - p2.x;
    int dy = p1.y - p2.y;

    if (abs(dx) > abs(dy))
    {
        Point left, right, curPoint;
        if (p1.x < p2.x)
        {
            left  = p1;
            right = p2;
        }
        else
        {
            left  = p2;
            right = p1;
        }

        if (dx > 0)
        {
            /* 每移动一个目标点的y方向累加量 */
            double delta_y  = (left.y - right.y) * 1.0 / abs(dx);
            /* y方向的点，定义时作为遍历的起点 */
            double double_y = right.y;

            for (int i = 0; i < dx; i ++)
            {
                int int_y = (double_y >= 0) ? (int)(double_y + 0.5) : (int)(double_y - 0.5);
                double_y += delta_y;
                curPoint = Point(right.x - i, int_y);
                                
                if (!canPass(curPoint, ref))
                {
                    mObstaclePoint = Point(curPoint.x, curPoint.y);
                    return false;
                }
            }
        }
        else
        {
            double delta_y  = (right.y - left.y) * 1.0 / abs(dx);
            double double_y = left.y;

            for (int i = 0; i < abs(dx); i ++)
            {
                int int_y = (double_y >= 0) ? (int)(double_y + 0.5) : (int)(double_y - 0.5);
                double_y += delta_y;
                curPoint = Point(left.x + i, int_y);

                if (!canPass(curPoint, ref))
                {
                    mObstaclePoint = Point(curPoint.x, curPoint.y);
                    return false;
                }
            }
        }
    }
    else 
    {
        Point top, down, curPoint;
		if (p1.y > p2.y)
		{
			top  = p1;
			down = p2;
		}
		else
		{
			top  = p2;
			down = p1;
		}

		if (dy > 0)
		{
			double delta_x  = (down.x - top.x) * 1.00 / abs(dy);
			double double_x = top.x;
			for (int i = 0; i < dy; i++)
			{
				int int_x = double_x >= 0 ? (int)(double_x + 0.5) : (int)(double_x - 0.5);
				double_x += delta_x;
				curPoint = Point(int_x, top.y - i);
         
				if (!canPass(curPoint, ref))
				{
					mObstaclePoint = Point(curPoint.x, curPoint.y);
					return false;
				}
			}
		}
		else
		{
			double delta_x  = (top.x - down.x)  * 1.00 / abs(dy);
			double double_x = down.x;
			for (int i = 0; i < abs(dy); i++)
			{
				int int_x = double_x >= 0 ? (int)(double_x + 0.5) : (int)(double_x - 0.5);
				double_x += delta_x;
				curPoint = Point(int_x,(int)( down.y + i));
             
				if (!canPass(curPoint, ref))
				{
					mObstaclePoint = Point(curPoint.x, curPoint.y);
					return false;
				}
			}
		}
    }

    return true;
}


/* End of file */
